#ifndef TARGET_TRACKER_2012
#define TARGET_TRACKER_2012

#include "Include.h"
#include "nivision.h"
#include "Target.h"
#include "MathExtension.h"
#include <vector>

namespace Team675
{

struct LaunchInfo {
	float LaunchSpeed;  // speed is inch per second
	float LaunchVerticalAngle;
	float HorizAngleError;
	float ConfidenceLevel;
	LaunchInfo() {
		LaunchSpeed = LaunchVerticalAngle = HorizAngleError = ConfidenceLevel = 0;
	}
	void Print() { 
		printf("Launch: s:%.2f va:%.2f he:%.2f\n", LaunchSpeed,
				LaunchVerticalAngle * DEGREES_PER_RADIAN,
				HorizAngleError * DEGREES_PER_RADIAN);

		SmartDashboard::GetInstance()->PutDouble("Launch vertical angle:",(double)LaunchVerticalAngle );
		SmartDashboard::GetInstance()->PutDouble("Launch Horiz angle:", (double)HorizAngleError);
		SmartDashboard::GetInstance()->PutDouble("Launch Speed:", (double)LaunchSpeed);
	}
};

struct Position
{
	float x;
	float z;
	float error;
	Position(){x = 0; z = 0; error = 0;}
	Position(float _x, float _z) { x = _x; z = _z; error = 0; }
	void Print() 
	{
		printf("(%.0f, %.0f)", x, z);
		SmartDashboard::GetInstance()->PutDouble("Robot pos x:", (double)x);
		SmartDashboard::GetInstance()->PutDouble("Robot Pos z:", (double)z);
	}
	void SetError(Position const &Avg)
	{
		float dx = (x - Avg.x);
		float dz = (z - Avg.z);
		error = sqrt((dx*dx) + (dz * dz));
	}
	
};

class TargetTracker2012 {
public:
	TargetTracker2012();
	LaunchInfo Update(Target::TargetPosition _TargetPosition, bool _swish);
	LaunchInfo Update(Target::TargetPosition _TargetPosition);
	LaunchInfo Update();
	
	static LaunchInfo GetLaunchVelocityNoCam(Position _robot, Target::TargetPosition _target, bool _swish);
private:
	Target m_targets[4];
	AxisCamera *m_camera;
	BinaryImage *m_binImage;
	int imgWidth, imgHeight; //width and height of the camera image in pixels
	Target::TargetPosition m_targetMode;
	int m_confidence;
	
	BinaryImage *ProcessImage(); //makes a BinaryImage that is ready for particle analysis
	Position GetRobotPosition();
	Position GetRobotPosition(Target::TargetPosition _targPos1, Target::TargetPosition _targPos2);
	Position GetRobotPosition(Target::TargetPosition _targPos);
	LaunchInfo GetLaunchVelocity(Position _robot, Target::TargetPosition _target, bool _swish);
	
	void CreateTarget(ParticleAnalysisReport *_report, Target::TargetPosition _position);
	
	float GetAzimuthFromWidth(Target::TargetPosition _targPos);
	float GetHorizAngle(int _pixelX, int _pixelY);
	float GetVertAngle(int _pixelX, int _pixelY);
	void GetDistAndHeight(ParticleAnalysisReport &_target, float &_dist, float &_height);
	float GetDist(float _height, float _angle);
	void GetTargets(vector<ParticleAnalysisReport>  particles);
	static float GetTargetX(Target::TargetPosition _targPos);
	static float GetTargetY(Target::TargetPosition _targPos);
};

}

#endif
